#include <QtGui/QApplication>
#include "mainwindow.h"
#include <stdlib.h>
#include <assert.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <vector>
#include <cmath>
#include <QMutex>
#include <iostream>
#include <time.h>

using namespace std;

static const bool DEBUG_FLAG = false;
static const bool PER_SECOND_FLAG = true;

MainWindow *guiWindow;

class MyThread_robot_thread : public QThread
{
public:
    void run()
    {
        time_t start_time = time(NULL);
        long long int loop_counter = 0;

        if (DEBUG_FLAG) printf("Entering robot thread.\n");

        while(guiWindow->start_or_end_program_flag == 1)
        {
            guiWindow->DC_motor_0->move_motor();
            if (DEBUG_FLAG) {
                float temp = guiWindow->DC_motor_0->my_DC_phidget_motor->commandVelocity;
                float temp2 = guiWindow->DC_motor_0->my_DC_phidget_motor->kp;
                float temp3 = guiWindow->DC_motor_0->my_DC_phidget_motor->kv;
                float temp4 = guiWindow->DC_motor_0->my_DC_phidget_motor->ki;
                printf("DC velocity: %f  kp: %f  kv: %f  ki: %f\n", temp, temp2, temp3, temp4);
            }

            for(int i = 0; i < guiWindow->servo_board->num_servos; i++)
            {
                guiWindow->servo_board->updatePosition(i);
                guiWindow->servo_board->move_motor(i, guiWindow->servo_board->goal_pos[i]);
            }

            if (PER_SECOND_FLAG) {
                loop_counter++;
                time_t current_time = time(NULL);

                if (loop_counter % 10000 == 0) {
                    cout << "Control iterations per second: " << (int)(loop_counter / difftime(current_time, start_time)) << endl;
                }
            }
        }

        if (DEBUG_FLAG) printf("Exiting robot thread.\n");
    }
};



int main(int argc, char *argv[])
{
    if (DEBUG_FLAG) cout << "Program entered." << endl;

    QApplication app(argc, argv);

    guiWindow = new MainWindow();
    guiWindow->setGeometry(50,50,1000,600);
    guiWindow->setAutoFillBackground(true);
    guiWindow->show();

    MyThread_robot_thread robot_thread;
    robot_thread.start(QThread::TimeCriticalPriority);
    
    return app.exec(); //runs qt program NECESSARY waits until the gui the thread is finished, which is when all the windows are closed

    robot_thread.wait();
}
